/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_
#define CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_

#include <deque>
#include <functional>
#include <limits>
#include <map>
#include <memory>
#include <set>
#include <vector>

#include "Eigen/Core"
#include "Eigen/Geometry"
#include "absl/container/flat_hash_map.h"
#include "absl/synchronization/mutex.h"
#include "cartographer/common/fixed_ratio_sampler.h"
#include "cartographer/common/thread_pool.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping/2d/submap_2d.h"
#include "cartographer/mapping/internal/constraints/constraint_builder_2d.h"
#include "cartographer/mapping/internal/optimization/optimization_problem_2d.h"
#include "cartographer/mapping/internal/pose_graph_data.h"
#include "cartographer/mapping/internal/trajectory_connectivity_state.h"
#include "cartographer/mapping/internal/work_queue.h"
#include "cartographer/mapping/pose_graph.h"
#include "cartographer/mapping/pose_graph_trimmer.h"
#include "cartographer/mapping/value_conversion_tables.h"
#include "cartographer/metrics/family_factory.h"
#include "cartographer/sensor/fixed_frame_pose_data.h"
#include "cartographer/sensor/landmark_data.h"
#include "cartographer/sensor/odometry_data.h"
#include "cartographer/sensor/point_cloud.h"
#include "cartographer/transform/rigid_transform.h"
#include "cartographer/transform/transform.h"

namespace cartographer
{
	namespace mapping
	{
		// Implements the loop closure method called Sparse Pose Adjustment (SPA) from
		// Konolige, Kurt, et al. "Efficient sparse pose adjustment for 2d mapping."
		// Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ International Conference
		// on (pp. 22--29). IEEE, 2010.
		//
		// It is extended for submapping:
		// Each node has been matched against one or more submaps (adding a constraint
		// for each match), both poses of nodes and of submaps are to be optimized.
		// All constraints are between a submap i and a node j.

		/*
		 * https://zhuanlan.zhihu.com/p/584648233
		 * https://zhuanlan.zhihu.com/p/364377540?utm_id=0
		 * 从SLAM角度，SPA优化是一种位姿图优化,从数学角度，SPA优化本质上是L-M优化算法;求解L-M优化的核心在于求解增量方程，由
		 * 于不是所有位姿节点间都有约束，故增量方程左侧的海塞矩阵具有明显的稀疏性，这允许我们通过技巧加速H的乔里斯基分解，使
		 * 得大规模优化问题的近实时求解成为可能;SPA优化的对象是所有节点的世界位姿, 在节点间构建相对位姿约束即边。
		 * cartographer中的节点共有两类(关键帧节点和子图节点)，关键帧指激光关键帧，而子图(submap)是由连续的若干个激光关键帧
		 * 拼接到一起形成的子地图,这两类节点都是优化对象，所有的约束（边）都是在这两类节点之间构建，同类节点之间不构建约束
		 *（暂且这么认为，实际稍有不同)。
		 */

		/*
		 * 主要三部分：
		 * 1) 接收来自Local SLAM的子图更新结果，在位姿图中相应的增加节点和约束
		 * 2) 结合里程计、IMU、全局位姿测量数据(Fixed Frame Pose, 类似GPS这样具有全局测量能力的传感器)的测量信息,使用SPA
		 *   (Sparse Pose Adjustment)技术求解后端的优化问题
		 * 3) 在每次后端优化后，根据优化的结果对后来添加到位姿图中的节点和子图的位姿做修正
		 */

		/* 一条轨迹包含多个节点，每个节点表示一个前端对扫描数据进行匹配的结果。有两种约束，子图内约束(INTRA_SUBMAP)
		 * 和子图间(INTER_SUBMAP)约束。INTRA_SUBMAP的约束是指在子图的更新过程中节点j被直接插入到子图i中。INTER_SUBMAP
		 * 类型的约束中节点j并不是直接插入到子图i中，猜测应该是因为闭环检测而添加的约束。
		 * 后端优化部分也有轨迹，节点，子图等数据容器，这些数据都是从前端匹配部分添加的，主要是用来做全局优化的。
		 * 所有要和前端的数据区分开来。
		 */

		/*
		 * https://blog.csdn.net/qq_41663016/article/details/130637263
		 * 1) 确定两个节点(前端一次匹配后的结果)在global坐标系下的相对位姿变换.
		 * 2) 通过其它方式再次获取这两个节点的相对位姿变化
		 * 3) 对这两个相对位姿变换的差的最小二乘问题进行求解
		 * 4) 进行求解后得到一个增量Δx, 将当前位姿加上这个增量就得到了优化后的位姿
		 *
		 * 使用的是ceres进行位姿图优化. global坐标系到后端部分才出现, 前端都是在local坐标系下做的.猜测第一次进入后端前,
		 * local坐标系和global坐标系是重合的, 进入后端后, local和global坐标系才会有偏移. 经测试, local坐标系在rviz中
		 * 是map, global中是odom.
		 *
		 * 残差：
		 * 1) 将节点(tracking的位姿)与节点(子图原点位姿)在global坐标系下的相对位姿与约束(包含子图内约束与子图间约束)的差值作为残差项.
		 * 2) landmark数据与通过2个节点位姿插值出来的相对位姿的差值作为残差项
		 * 3) 节点与节点间在global坐标系下的相对坐标变换 与 通过里程计数据插值出的相对坐标变换的差值作为残差项
		 * 4) 节点与节点间在global坐标系下的相对坐标变换 与 相邻2个节点在local坐标系下的相对坐标变换的差值作为残差项
		 * 5) 节点与gps坐标系原点在global坐标系下的相对坐标变换 与 通过gps数据进行插值得到的相对坐标变换 的差值作为残差项
		 */
		class PoseGraph2D : public PoseGraph
		{
		public:
			PoseGraph2D(
				const proto::PoseGraphOptions &options,
				std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem,
				common::ThreadPool *thread_pool);
			~PoseGraph2D() override;

			PoseGraph2D(const PoseGraph2D &) = delete;
			PoseGraph2D &operator=(const PoseGraph2D &) = delete;

			// Adds a new node with 'constant_data'. Its 'constant_data->local_pose' was
			// determined by scan matching against 'insertion_submaps.front()' and the
			// node data was inserted into the 'insertion_submaps'. If
			// 'insertion_submaps.front().finished()' is 'true', data was inserted into
			// this submap for the last time.
			NodeId AddNode(
				std::shared_ptr<const TrajectoryNode::Data> constant_data,
				int trajectory_id,
				const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps)
				LOCKS_EXCLUDED(mutex_);

			void AddImuData(int trajectory_id, const sensor::ImuData &imu_data) override
				LOCKS_EXCLUDED(mutex_);

			void AddOdometryData(int trajectory_id,
								 const sensor::OdometryData &odometry_data) override
				LOCKS_EXCLUDED(mutex_);

			void AddFixedFramePoseData(
				int trajectory_id,
				const sensor::FixedFramePoseData &fixed_frame_pose_data) override
				LOCKS_EXCLUDED(mutex_);

			void AddLandmarkData(int trajectory_id,
								 const sensor::LandmarkData &landmark_data) override
				LOCKS_EXCLUDED(mutex_);

			void DeleteTrajectory(int trajectory_id) override;

			void FinishTrajectory(int trajectory_id) override;

			bool IsTrajectoryFinished(int trajectory_id) const override
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			void FreezeTrajectory(int trajectory_id) override;
			bool IsTrajectoryFrozen(int trajectory_id) const override
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);
			void AddSubmapFromProto(const transform::Rigid3d &global_submap_pose,
									const proto::Submap &submap) override;
			void AddNodeFromProto(const transform::Rigid3d &global_pose,
								  const proto::Node &node) override;
			void SetTrajectoryDataFromProto(const proto::TrajectoryData &data) override;
			void AddNodeToSubmap(const NodeId &node_id,
								 const SubmapId &submap_id) override;
			void AddSerializedConstraints(
				const std::vector<Constraint> &constraints) override;
			void AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer) override;
			void RunFinalOptimization() override;
			std::vector<std::vector<int>> GetConnectedTrajectories() const override
				LOCKS_EXCLUDED(mutex_);
			PoseGraphInterface::SubmapData GetSubmapData(const SubmapId &submap_id) const
				LOCKS_EXCLUDED(mutex_) override;
			MapById<SubmapId, PoseGraphInterface::SubmapData> GetAllSubmapData() const
				LOCKS_EXCLUDED(mutex_) override;
			MapById<SubmapId, SubmapPose> GetAllSubmapPoses() const
				LOCKS_EXCLUDED(mutex_) override;
			transform::Rigid3d GetLocalToGlobalTransform(int trajectory_id) const
				LOCKS_EXCLUDED(mutex_) override;
			MapById<NodeId, TrajectoryNode> GetTrajectoryNodes() const override
				LOCKS_EXCLUDED(mutex_);
			MapById<NodeId, TrajectoryNodePose> GetTrajectoryNodePoses() const override
				LOCKS_EXCLUDED(mutex_);
			std::map<int, TrajectoryState> GetTrajectoryStates() const override
				LOCKS_EXCLUDED(mutex_);
			std::map<std::string, transform::Rigid3d> GetLandmarkPoses() const override
				LOCKS_EXCLUDED(mutex_);
			void SetLandmarkPose(const std::string &landmark_id,
								 const transform::Rigid3d &global_pose,
								 const bool frozen = false) override
				LOCKS_EXCLUDED(mutex_);
			sensor::MapByTime<sensor::ImuData> GetImuData() const override
				LOCKS_EXCLUDED(mutex_);
			sensor::MapByTime<sensor::OdometryData> GetOdometryData() const override
				LOCKS_EXCLUDED(mutex_);
			sensor::MapByTime<sensor::FixedFramePoseData> GetFixedFramePoseData()
				const override LOCKS_EXCLUDED(mutex_);
			std::map<std::string /* landmark ID */, PoseGraph::LandmarkNode>
			GetLandmarkNodes() const override LOCKS_EXCLUDED(mutex_);
			std::map<int, TrajectoryData> GetTrajectoryData() const override
				LOCKS_EXCLUDED(mutex_);
			std::vector<Constraint> constraints() const override LOCKS_EXCLUDED(mutex_);
			void SetInitialTrajectoryPose(int from_trajectory_id, int to_trajectory_id,
										  const transform::Rigid3d &pose,
										  const common::Time time) override
				LOCKS_EXCLUDED(mutex_);
			void SetGlobalSlamOptimizationCallback(
				PoseGraphInterface::GlobalSlamOptimizationCallback callback) override;
			transform::Rigid3d GetInterpolatedGlobalTrajectoryPose(
				int trajectory_id, const common::Time time) const
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			static void RegisterMetrics(metrics::FamilyFactory *family_factory);

		private:
			MapById<SubmapId, PoseGraphInterface::SubmapData> GetSubmapDataUnderLock()
				const EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Handles a new work item.
			void AddWorkItem(const std::function<WorkItem::Result()> &work_item)
				LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);

			// Adds connectivity and sampler for a trajectory if it does not exist.
			void AddTrajectoryIfNeeded(int trajectory_id)
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Appends the new node and submap (if needed) to the internal data
			// structures.
			NodeId AppendNode(
				std::shared_ptr<const TrajectoryNode::Data> constant_data,
				int trajectory_id,
				const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps,
				const transform::Rigid3d &optimized_pose) LOCKS_EXCLUDED(mutex_);

			// Grows the optimization problem to have an entry for every element of
			// 'insertion_submaps'. Returns the IDs for the 'insertion_submaps'.
			std::vector<SubmapId> InitializeGlobalSubmapPoses(
				int trajectory_id, const common::Time time,
				const std::vector<std::shared_ptr<const Submap2D>> &insertion_submaps)
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Adds constraints for a node, and starts scan matching in the background.
			WorkItem::Result ComputeConstraintsForNode(
				const NodeId &node_id,
				std::vector<std::shared_ptr<const Submap2D>> insertion_submaps,
				bool newly_finished_submap) LOCKS_EXCLUDED(mutex_);

			// Computes constraints for a node and submap pair.
			void ComputeConstraint(const NodeId &node_id, const SubmapId &submap_id)
				LOCKS_EXCLUDED(mutex_);

			// Deletes trajectories waiting for deletion. Must not be called during
			// constraint search.
			void DeleteTrajectoriesIfNeeded() EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Runs the optimization, executes the trimmers and processes the work queue.
			void HandleWorkQueue(const constraints::ConstraintBuilder2D::Result &result)
				LOCKS_EXCLUDED(mutex_) LOCKS_EXCLUDED(work_queue_mutex_);

			// Process pending tasks in the work queue on the calling thread, until the
			// queue is either empty or an optimization is required.
			void DrainWorkQueue() LOCKS_EXCLUDED(mutex_)
				LOCKS_EXCLUDED(work_queue_mutex_);

			// Waits until we caught up (i.e. nothing is waiting to be scheduled), and
			// all computations have finished.
			void WaitForAllComputations() LOCKS_EXCLUDED(mutex_)
				LOCKS_EXCLUDED(work_queue_mutex_);

			// Runs the optimization. Callers have to make sure, that there is only one
			// optimization being run at a time.
			void RunOptimization() LOCKS_EXCLUDED(mutex_);

			bool CanAddWorkItemModifying(int trajectory_id)
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Computes the local to global map frame transform based on the given
			// 'global_submap_poses'.
			transform::Rigid3d ComputeLocalToGlobalTransform(
				const MapById<SubmapId, optimization::SubmapSpec2D> &global_submap_poses,
				int trajectory_id) const EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			SubmapData GetSubmapDataUnderLock(const SubmapId &submap_id) const
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			common::Time GetLatestNodeTime(const NodeId &node_id,
										   const SubmapId &submap_id) const
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			// Updates the trajectory connectivity structure with a new constraint.
			void UpdateTrajectoryConnectivity(const Constraint &constraint)
				EXCLUSIVE_LOCKS_REQUIRED(mutex_);

			const proto::PoseGraphOptions options_;
			// 完成全局优化后的回调函数
			GlobalSlamOptimizationCallback global_slam_optimization_callback_;
			mutable absl::Mutex mutex_;
			absl::Mutex work_queue_mutex_;

			// If it exists, further work items must be added to this queue, and will be considered later.
			// 相当于一个缓存buffer，将要处理的数据缓存
			std::unique_ptr<WorkQueue> work_queue_ GUARDED_BY(work_queue_mutex_);

			// We globally localize a fraction of the nodes from each trajectory.
			absl::flat_hash_map<int, std::unique_ptr<common::FixedRatioSampler>>
				global_localization_samplers_ GUARDED_BY(mutex_);

			// Number of nodes added since last loop closure.
			// 记录了自从上次闭环检测后新增的节点数量
			int num_nodes_since_last_loop_closure_ GUARDED_BY(mutex_) = 0;

			// Current optimization problem.
			std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem_;

			constraints::ConstraintBuilder2D constraint_builder_;

			// Thread pool used for handling the work queue.
			common::ThreadPool *const thread_pool_;

			// List of all trimmers to consult when optimizations finish.
			std::vector<std::unique_ptr<PoseGraphTrimmer>> trimmers_ GUARDED_BY(mutex_);

			PoseGraphData data_ GUARDED_BY(mutex_);

			ValueConversionTables conversion_tables_;

			// Allows querying and manipulating the pose graph by the 'trimmers_'. The
			// 'mutex_' of the pose graph is held while this class is used.
			class TrimmingHandle : public Trimmable
			{
			public:
				TrimmingHandle(PoseGraph2D *parent);
				~TrimmingHandle() override {}

				int num_submaps(int trajectory_id) const override;
				std::vector<SubmapId> GetSubmapIds(int trajectory_id) const override;
				MapById<SubmapId, SubmapData> GetOptimizedSubmapData() const override
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
				const MapById<NodeId, TrajectoryNode> &GetTrajectoryNodes() const override
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
				const std::vector<Constraint> &GetConstraints() const override
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
				void TrimSubmap(const SubmapId &submap_id)
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_) override;
				bool IsFinished(int trajectory_id) const override
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);
				void SetTrajectoryState(int trajectory_id, TrajectoryState state) override
					EXCLUSIVE_LOCKS_REQUIRED(parent_->mutex_);

			private:
				PoseGraph2D *const parent_;
			};
		};

	} // namespace mapping
} // namespace cartographer

#endif // CARTOGRAPHER_MAPPING_INTERNAL_2D_POSE_GRAPH_2D_H_
